#include "usr_inc.h"

/* NRF点位信息的缓存区 */
PointMsg_t nrfPoint = {0};

/* 按键和串口屏数据会连续发送10次, 因此定义一个接收标志位 */
FunctionalState getFlag = DISABLE;

/**
 * @brief 限制摇杆数据
 */
int16_t NRF_LimitJoy(int16_t val)
{
    if (abs(val) <= JOY_DZ) return 0;

    val = (val > 0) ? (val - JOY_DZ) : (val + JOY_DZ);

    if (val > JOY_MAX) return JOY_MAX;
    if (val < -JOY_MAX) return -JOY_MAX;

    return val;
}

/**
 * @brief 初始化NRF
 */
void ALL_NRF_Init(void)
{
    while (NRF24L01_Check())
    {
        ;
    }
    NRF24L01_Init_Rx();
}

void StartNrfUpdate(void *argument)
{
    for (uint16_t period = 1; ;)
    {
        uint8_t data[9];
        const uint8_t res = NRF_Recv(data);

        /* 摇杆数据 */
        if (res == 0x02)
        {
            getFlag = DISABLE;

            int16_t lx = (((uint16_t)data[2] & 0x0F) << 8) | data[1]; // 设置的是输出12bit, 因此需要把16bit的高4位去掉, 所以需要&0x0F
            int16_t ly = (((uint16_t)data[4] & 0x0F) << 8) | data[3];
            int16_t ry = (((uint16_t)data[6] & 0x0F) << 8) | data[5];
            int16_t rx = (((uint16_t)data[8] & 0x0F) << 8) | data[7];

            JoyStickMsg_t msg = {0};
            msg.leftX = NRF_LimitJoy(-lx + 2100);
            msg.leftY = NRF_LimitJoy(ly - 2000);
            msg.rightX = NRF_LimitJoy(-rx + 2100);
            msg.rightY = NRF_LimitJoy(ry - 2000);

            xQueueSendToBack(joyMsgQueueHandle, &msg, 1);
        }
        /* 按键数据 */
        else if (res == 'K' && getFlag == DISABLE)
        {
            uint8_t key = data[1];
            xQueueSendToBack(keyCmdQueueHandle, &key, 1);

            getFlag = ENABLE;
        }
        /* 推射指令 */
        else if (res == 0xAA && getFlag == DISABLE)
        {
            if (data[1] == '>')
            {
                TossCmdMsg_t cmd;
                if (data[2] == 'L')
                {
                    cmd = TOSS_DRAG;
                    xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'F')
                {
                    cmd = TOSS_FREE;
                    xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'N')
                {
                    cmd = TOSS_ABANDON;
                    xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'S')
                {
                    cmd = TOSS_UP;
                    xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'X')
                {
                    cmd = TOSS_DOWN;
                    xQueueSendToBack(tossCmdQueueHandle, &cmd, 1);
                }
            }
            getFlag = ENABLE;
        }
        /* 运球指令 */
        else if (res == 0xAC && getFlag == DISABLE)
        {
            if (data[1] == ']')
            {
                DribbleCmdMsg_t cmd;
                if (data[2] == 'S')
                {
                    cmd = DRIBBLE_EXTEND;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'H')
                {
                    cmd = DRIBBLE_RETRACT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'X')
                {
                    cmd = DRIBBLE_DRAW;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'L')
                {
                    cmd = DRIBBLE_FREE;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'P')
                {
                    cmd = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'W')
                {
                    cmd = DRIBBLE_HOLD;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd, 1);
                }
            }
            getFlag = ENABLE;
        }
        /* 推程数据 */
        else if (res == 0xDE && getFlag == DISABLE)
        {
            int32_t rawS;
            memcpy(&rawS, data + 1, sizeof(int32_t));

            float stroke = (float)rawS / 1000000.0;
            xQueueSendToBack(tossStrokeQueueHandle, &stroke, 1);

            getFlag = ENABLE;
        }
        /* 底盘指令 */
        else if (res == 0xBB && getFlag == DISABLE)
        {
            if (data[1] == 'C')
            {
                ChassisCmdMsg_t cmd;
                if (data[2] == 'L')
                {
                    cmd = CHASSIS_LOCK;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'F')
                {
                    cmd = CHASSIS_FREE;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'Y')
                {
                    cmd = CHASSIS_CONF_YAW;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'J')
                {
                    cmd = CHASSIS_CONF_JOY;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'P')
                {
                    cmd = CHASSIS_CONF_POINT;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
            }
            getFlag = ENABLE;
        }
        /* 底盘点位信息 */
        else if (res == 0xBC && getFlag == DISABLE)
        {
            int32_t rawX, rawY;
            memcpy(&rawX, data + 1, sizeof(int32_t));
            memcpy(&rawY, data + 5, sizeof(int32_t));

            nrfPoint.posX = (float)rawX / 1000.0;
            nrfPoint.posY = (float)rawY / 1000.0;
            nrfPoint.sum = rawX + rawY;

            getFlag = ENABLE;
        }
        else if (res == 0xBD && getFlag == DISABLE)
        {
            int32_t rawYaw, rawSum;
            memcpy(&rawYaw, data + 1, sizeof(int32_t));
            memcpy(&rawSum, data + 5, sizeof(int32_t));

            float yaw = (float)rawYaw / 1000.0;
            nrfPoint.sum += rawYaw;

            if (nrfPoint.sum == rawSum)
            {
                xQueueSendToBack(chassisPointCmdQueueHandle, &nrfPoint, 1);
                xQueueSendToBack(chassisYawCmdQueueHandle, &yaw, 1);
            }

            memset(&nrfPoint, 0, sizeof(PointMsg_t));

            getFlag = ENABLE;
        }
        /* 对抗赛指令 */
        else if (res == 0x1B && getFlag == DISABLE)
        {
            if (data[1] == 'V')
            {
                ChassisCmdMsg_t cmd;
                if (data[2] == 'S')
                {
                    cmd = CHASSIS_LOW_SPD;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'M')
                {
                    cmd = CHASSIS_MID_SPD;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'H')
                {
                    cmd = CHASSIS_HIGH_SPD;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
                }
            }
            else if (data[1] == '*')
            {
                BattleCmdMsg_t cmd;
                AimMode_t mode;
                if (data[2] == 'Y')
                {
                    cmd = BATTLE_PREPARE;
                    xQueueSendToBack(battleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'C')
                {
                    cmd = BATTLE_GO;
                    xQueueSendToBack(battleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'H')
                {
                    mode = AIM_HOOP;
                    xQueueSendToBack(aimModeQueueHandle, &mode, 1);
                }
                else if (data[2] == 'R')
                {
                    mode = AIM_R2;
                    xQueueSendToBack(aimModeQueueHandle, &mode, 1);
                }
                else if (data[2] == 'S')
                {
                    cmd = BATTLE_SHOOT;
                    xQueueSendToBack(battleCmdQueueHandle, &cmd, 1);
                }
                else if (data[2] == 'Z')
                {
                    cmd = BATTLE_RESET;
                    xQueueSendToBack(battleCmdQueueHandle, &cmd, 1);
                }
            }
            getFlag = ENABLE;
        }

        vTaskDelay(pdMS_TO_TICKS(period));
    }
}
